/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *                      
 * FILENAME:
 * matrix.h
 *
 * PURPOSE:
 * To implement matrix objects and the needed math
 *
 * CREATED:
 * 02/2005 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Date: 2005/07/13 21:00:06 $
 * $Revision: 1.3 $
 * $Author: dixonc $
 ***********************************************************************/ 

#ifndef __cplusplus
#	error Must use C++ for the type Matrix.
#endif

#ifndef _MATRIX_H
#define _MATRIX_H
#define EPS 1e-16

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>

using namespace std;

//////////////////////////////////////////////////////////////////////
// Macros to acces command data out of a char buffer array
//  use these very CAREFULLY
//////////////////////////////////////////////////////////////////////
#define RET_MIN(a,b) ( a < b ? a : b )
#define RET_MAX(a,b) ( a > b ? a : b )

#define INDALL int(-1);

// you can change this to float/double or to a template
typedef double Matrix_t;

/*
 * The matrix class provides the necessary matrix structure and
 * math necessary for such things as transormations, propogation, ...
 * This class contains code for most of the known Matrix functions.
 * Matrices can be read from and saved to a file, with only one matrix
 * per file.  
 */
class Matrix
{
public:
// Constructors for the Matrix class 
//
// Default Constructor
//	 param row number of rows
//	 param col number of columns
//	 param v value(s) to fill in array
// The default constructors can be used in the following ways:
// <pre>
//    Matrix a(3, 3, 3.14);   
//    Matrix a(3, 3);          
//    float val[9] = { 1, 2, 3, 4, 5, 6, 7, 8, 9};
//    Matrix a(3, 3, val);      
// </pre>
	Matrix (size_t row = 0, size_t col = 0, Matrix_t v = Matrix_t(0) );
	Matrix (size_t row, size_t col, Matrix_t v[] );
	Matrix (size_t row, size_t col, float v[] );

// Copy Constructor
// 	param m Matrix to copy
	Matrix (const Matrix& m);

// Load a matrix from a file.
// If the file is not found, then an empty matrix is returned 
// 	param fileName name of file to load matrix from
	Matrix (char* fileName);

// Destructor
	~Matrix ();

// 
// Friend functions to other classes
//
	// output a matrix to a stream
	friend ostream& operator << (ostream& ostrm, const Matrix& m);
	// input a matrix from a stream
	friend istream& operator >> (istream& istrm, Matrix& m);
	// multiply a matrix with a type Matrix_t var
	friend Matrix operator * (const Matrix_t& no, const Matrix& m);
	// divide a matrix with a type Matrix_t var
	friend Matrix operator / (const Matrix_t& no, const Matrix& m);

//
// Assignment & Equality
// Set or test for Assignment & Equality
//
	// Assignment operators
	Matrix& operator = (const Matrix& m);
	Matrix& operator = (const Matrix_t inputArray[]);

	// Test for equality of a matrix by elements 
	bool operator == (const Matrix& m) const;
	// Test for non-equality of a matrix by elements 
	bool operator != (const Matrix& m) const;

//
// Subscript Operators
// Get individual elements, rows, or columns
//
	// get the number of rows
	size_t NumRow () const { return Row; }
	// get the number of cols
	size_t NumCol () const { return Col; }
	// get the length
	size_t length () const { return (Row < Col) ? Col : Row; }
	// get the size
	size_t size () const { return Row * Col; }
	// row and column
	Matrix_t& operator () (size_t row, size_t col);
	// row and column
	Matrix_t  operator () (size_t row, size_t col) const;
	// straight index, for a matrix traverse row first
	Matrix_t  operator () (size_t num) const;
	// straight index, for a matrix traverse row first
	Matrix_t&  operator () (size_t num) ;

// Get and Set operations
	void getVec(Matrix_t vec[]);

	// get a column
	void getCol(size_t ind, Matrix_t vec[]);
	Matrix getCol(size_t ind);
	// set a column
	void setCol(size_t ind, Matrix_t vec[]);
	void setCol(size_t ind, Matrix_t val);
	void setCol(size_t ind, Matrix vec);
	// get a row
	void getRow(size_t ind, Matrix_t vec[]);
	Matrix getRow(size_t ind);
	// set a row
	void setRow(size_t ind, Matrix_t vec[]);
	void setRow(size_t ind, Matrix_t val);
	void setRow(size_t ind, Matrix vec);


// Math Operators
// Matrix & Vector Math
	// unary member operator function
	Matrix operator - () const;
	Matrix operator ! () const;
	Matrix operator ~ () const;

	// binary operators
	Matrix operator + (const Matrix& m2) const;
	Matrix operator - (const Matrix& m2) const;
	Matrix operator * (const Matrix& m2) const;
	Matrix operator / (const Matrix& m2) const;
	Matrix operator * (const Matrix_t& no) const;
	Matrix operator / (const Matrix_t& no) const;
	Matrix operator ^ (const size_t& no) const;

	// Combined assignment - calculation operators
	Matrix& operator += (const Matrix& m) ;
	Matrix& operator -= (const Matrix& m) ;
	Matrix& operator *= (const Matrix& m) ;
	Matrix& operator *= (const Matrix_t& c) ;
	Matrix& operator /= (const Matrix_t& c) ;
	Matrix& operator ^= (const size_t& pow) ;

	// make the matrix all zeros
	void Null () ;
	// make the matrix all ones
	void Ones () ;
	// make the matrix all zeros and resize
	void Null (const size_t& row, const size_t& col) ;
	// make this identity matrix
	void Identity () ;	
	// make this identity matrix and resize
	void Identity (const size_t& row) ;
	// set the size of the matrix, values are undefined after a resize
	void SetSize (size_t row, size_t col) ;

	// calculate the inverse
	Matrix Inv () const ;
	// solve the inverse problem given the matrix v,
	//  which should have proper dimension of a row vector
	//  with the dimension of the matrix
	Matrix Solve (const Matrix& v) const ;
	// calculate the adjoint
	Matrix Adj () const;
	// calculate the determinent
	Matrix_t Det () const ;
	// calculate the cofactor
	Matrix_t Cofact (size_t row, size_t col) const ;
	// calculate the condition number
	Matrix_t Cond () const;
	// calculate the sum of all elements
	Matrix_t Sum() const;
	// calclate the Cholesky decomposition using the Cholesky-Banachiewicz and Cholesky-Crout algorithms
	Matrix Chol() const;
	// calclate the upper triangular Cholesky decomposition
	Matrix Chol_up() const;
	// return the diagonal elements in a vector
	Matrix Diag() const;
	// set the diagonal elements from a vector
	void Diag(Matrix vec);
	// return sqrt of matrix elements
	Matrix Sqrt() const;

  // return the matrix square root
	Matrix mSqrt() const;

	// return maximum element
	Matrix_t Max() const;
	// return index of maximum element
	int MaxInd() const;
	// return minimum element
	Matrix_t Min() const;
	// return index of maximum element
	int MinInd() const;
	// return the trace of the matrix, which is the
	// sum of the diagonal
	Matrix_t Trace() const;

	// vector math functions, these don't make sense for matrices
	Matrix_t Std() const;
	Matrix_t Mean() const;
	Matrix_t Norm () const ;	
	Matrix_t Norm2 () const ;	

	Matrix   ColNorm () const ;	
	Matrix   ColNorm2 () const ;	

// Matrix Types
// memo Determine or set the type of the matrix
	bool IsSquare () const { return (Row == Col); } 
	bool IsSingular () const;
	bool IsDiagonal () const;
	bool IsScalar () const;
	bool IsIdentity () const;
	bool IsNull () const;
	bool IsUnit () const;
	bool IsSymmetric () const;
	bool IsSkewSymmetric () const;
	bool IsUpperTriangular () const;
	bool IsLowerTriangular () const;
	// set this vector ( or elements ) to unit length
	Matrix& Unit () ;
	// make skew-symmetric matrix from vector
	Matrix Skew() ;

// File Functions
// memo File input/output
	int fillFromFile(const char *file);
	int saveToFile(const char *file);

private:
	// the memory
	Matrix_t **Val;
	// number of rows
	size_t Row;
	// number of columns
	size_t Col;

	void realloc (size_t row, size_t col);
	int pivot (size_t row);
  Matrix_t pythag(const Matrix_t a, const Matrix_t b) const;
};


#endif //_MATRIX_H
